#ifndef BASICPCL_H_
#define  BASICPCL_H_

#include "Basicpcl.h"
IMPSizectorS *Sensor = MPSizectorS_Factory::GetInstance(MPSizectorS_LogMediaType_CallBack);//获取一个新的MPSizectorS实例。
clock_t Start_T;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
//相机初始化  获取点云  
int sxinit(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_camera){

 
    int choice;
	int DeviceCount = 0;
	bool rtv;
	Sensor->UpdateDeviceList();

	MPSizectorS_DeviceInfoStructType DeviceInfo;


	//check device count
	DeviceCount = Sensor->GetDeviceCount();
	if (DeviceCount <= 0) 
	{
		printf("No Device Found!\r\n");
		delete Sensor;
		system("pause");
		return 0;
	}


	//----------------------------------------------------------------
	///print device list
	printf("Found %d Devices:\r\n\r\n", DeviceCount);
	for(int i = 0; i< DeviceCount; i++)
	{
		DeviceInfo = Sensor->GetDeviceInfo(i);
		printf("Device #%d :\r\n", i);
#ifndef _WIN32
        printf("  -Name : %ls\r\n", DeviceInfo.DeviceName);
		printf("  -Serial Number : %ls\r\n", DeviceInfo.DeviceSerialNumber);
#else
        wprintf(L"  -Name : %s\r\n", DeviceInfo.DeviceName);
		wprintf(L"  -Serial Number : %s\r\n", DeviceInfo.DeviceSerialNumber);
#endif
		printf("  -Is Activated : %s\r\n", DeviceInfo.IsActivated ? "True" : "False");
		printf("  -HW Version : %.2f\r\n", (float)DeviceInfo.HWVersion);
		printf("  -FW Version : %.2f\r\n", (float)DeviceInfo.FWVersion);
		printf("  -Minimum SDK Version : %.2f\r\n", DeviceInfo.MinimumSDKVersion);
		printf("  -Sensor Width : %d\r\n", DeviceInfo.SensorWidth);
		printf("  -Sensor Height : %d\r\n", DeviceInfo.SensorHeight);
		printf("  -Sensor Resolution : %d\r\n", DeviceInfo.SensorResolution);
		printf("  -Binning Possibility : %s\r\n", DeviceInfo.BinningPossibility ? "True" : "False");
		printf("  -Sensor Binning Width : %d\r\n", DeviceInfo.SensorBinningWidth);
		printf("  -Sensor Binning Height : %d\r\n", DeviceInfo.SensorBinningHeight);
		printf("  -Sensor Binning Resolution : %d\r\n", DeviceInfo.SensorBinningResolution);
		printf("  -X Max : %.2f\r\n", DeviceInfo.XMax);
		printf("  -Y Max : %.2f\r\n", DeviceInfo.YMax);
		printf("  -Z Max : %.2f\r\n", DeviceInfo.ZMax);
		printf("  -Device Interface : %s\r\n", (DeviceInfo.InterfaceType == MPSizectorS_InterfaceType_U2) ? "USB2.0" : ((DeviceInfo.InterfaceType == MPSizectorS_InterfaceType_U3) ? "USB3.0" : "EtherNet"));

	}
	//------------------------------------------------------------------------------------------

	//--------------------------------------------------------------------------------------------
	///set callback 设置日志回调函数。
	if (SHOW_LOG) Sensor->SetLogCallBack(LogCallBack);
	Sensor->SetDeviceStateChangeCallBack(&DeviceStateChangeCallBack);
	Sensor->SetDataCallBack(DataCallBack);
	//-----------------*************************************************************************************
	//open device
	printf("\r\nEnter device index to open: ");
	scanf_s("%d", &choice);
	//choice = 0;
	rtv = Sensor->Open(Sensor->GetDeviceInfo(choice));//获取设备列表中指定Index的设备信息。 
	if (!rtv) 
	{
		printf("\r\nOpen device failed.\r\n\r\n");
		delete Sensor;
		system("pause");
		return 0;
	}
	printf("\r\nOpen device successfully.\r\n\r\n");
	//------------------------------------------------------------------------------------
	//basic setting
	Sensor->SetTriggerSource(MPSizectorS_TriggerSourceType_SoftTriggerOnly); //设定触发源。仅接收软件触发。
	Sensor->SetHoldState(false);//设定暂停状态。 
	Sensor->SetAutoReconnect(true);//设置自动重连功能的使能状态。自动重连功能默认使能。
	//Sensor->SetExposureIntensity2D(10);//设定2D拍摄曝光强度
	Sensor->SetExposureIntensity3D_1st(5);//设定3D拍摄第1曝光强度。
	Sensor->SetExposureIntensity3D_2nd(10);
	Sensor->SetExposureIntensity3D_3rd(20);
	Sensor->SetUserGain(0);//设定用户增益。 

	//wait for init done
	printf("Start waiting for device ready...");//获取设备的当前状态。 GetDeviceState
	while((Sensor->GetDeviceState() == MPSizectorS_DeviceStateType_Disconnected) || (Sensor->GetDeviceState() == MPSizectorS_DeviceStateType_UnderInit)) 
	{
		//Sleep(50);
		printf(".");
	}
	printf("Done\r\n\r\n");
	//MPSizectorS_DeviceStateType_Disconnected
	//	设备未连接。

	//	MPSizectorS_DeviceStateType_UnderInit
	//	设备正在初始化。

	//	MPSizectorS_DeviceStateType_StandBy
	//	设备处于待命状态，可进行一次新的拍摄。

	//	MPSizectorS_DeviceStateType_UnderExposure
	//	设备正在曝光。

	//	MPSizectorS_DeviceStateType_UnderTransfer
	//	设备正在传输数据。




//choose demo mode by several steps

//step 1, set binning mode
SetBinningModePos://设定基础设置。 

	choice =1;
	// printf("Choose Binning Choice:\r\n");
	// printf("1: Standard\r\n");
	// printf("2: Binning\r\n");
	// printf("0: Abort.\n");

	// scanf_s("%d", &choice);

	switch(choice)
	{
	case 1:
		Sensor->SetBinningState(false); //设定像素合并使能状态。 SetBinningState
		break;
	case 2:
		Sensor->SetBinningState(true);
		break;
	case 0:
		goto ExitPos;
		break;
	default:
		printf("Invalid input, please enter again. \n\n");
		goto SetBinningModePos;
		break;
	}


//setp 2, set data format  设定工作模式。 SetWorkingMode
SetDataFormat:

	choice =2;
	// printf("Choose Data Format:\r\n");
	// //printf("1: 2D Image\r\n");
	// printf("2: Float Point Cloud\r\n");
	// printf("3: Fix Point Cloud\r\n");
	// printf("0: Abort.\n");
	
	// scanf_s("%d", &choice);

	switch(choice)
	{
	//case 1:
		//Sensor->SetWorkingMode(MPSizectorS_WorkingModeType_2D_White);//全白照明2D模式。
		//goto StartSnapPose;
		//break;
	case 2:
		Sensor->SetDataOutMode(MPSizectorS_DataOutModeType_FloatPointCloud);//浮点数点云3D数据模式。X,Y,Z分别通过32位浮点数表示。
		break;
	case 3:
		Sensor->SetDataOutMode(MPSizectorS_DataOutModeType_FixPointCloud);//定点数点云3D数据模式。X,Y,Z分别通过16位定点数表示。 
		break;
	case 0:
		goto ExitPos;
		break;
	default:
		printf("Invalid input, please enter again. \n\n");
		goto SetDataFormat;
		break;
	}

//step 3, set 3d mode
Set3DMode:

	choice =3;
	// printf("Choose 3D Mode:\r\n");
	// printf("1: Fastest Manual Single Exposure\r\n");
	// printf("2: Finest Manual Single Exposure\r\n");
	// printf("3: Fastest AutoPHDR\r\n");
	// printf("4: Finest AutoPHDR\r\n");
	// printf("0: Abort.\n");

	// scanf_s("%d", &choice);

	
	MPSizectorS_ExposureBasicSettingStructType ExposureBasicSetting; //基础曝光设置。

	switch(choice)
	{
	case 1:
		Sensor->SetWorkingMode(MPSizectorS_WorkingModeType_3D_Fast);//快速3D模式。
		Sensor->SetUserGain(3);//设定用户增益。
		
		ExposureBasicSetting.ExposureMode = MPSizectorS_ExposureModeType_Manual;//手动曝光模式。采用固定的曝光次数和曝光强度。在3D工作模式下，当曝光次数大于1，3D数据为HDR融合的结果。 
		ExposureBasicSetting.ExposureNumber = 1;
		ExposureBasicSetting.AutoHDRPriority = 3;
		ExposureBasicSetting.AutoPHDRQuality = 90;
		Sensor->SetExposureBasicSetting(ExposureBasicSetting);
		
		break;
	case 2:
		Sensor->SetWorkingMode(MPSizectorS_WorkingModeType_3D_SuperPrecise);
		Sensor->SetUserGain(0);
		
		ExposureBasicSetting.ExposureMode = MPSizectorS_ExposureModeType_Manual;
		ExposureBasicSetting.ExposureNumber = 1;
		ExposureBasicSetting.AutoHDRPriority = 3;
		ExposureBasicSetting.AutoPHDRQuality = 90;
		Sensor->SetExposureBasicSetting(ExposureBasicSetting);
		
		break;
	case 3:
		Sensor->SetWorkingMode(MPSizectorS_WorkingModeType_3D_Fast);
		Sensor->SetUserGain(3);
		
		ExposureBasicSetting.ExposureMode = MPSizectorS_ExposureModeType_AutoPHDR;//自动PHDR曝光模式。设备根据指定的3D成像质量阈值 
		ExposureBasicSetting.ExposureNumber = 1;
		ExposureBasicSetting.AutoHDRPriority = 3;
		ExposureBasicSetting.AutoPHDRQuality = 90;
		Sensor->SetExposureBasicSetting(ExposureBasicSetting);
		
		break;
	case 4:
		Sensor->SetWorkingMode(MPSizectorS_WorkingModeType_3D_SuperPrecise);
		Sensor->SetUserGain(0);
		
		ExposureBasicSetting.ExposureMode = MPSizectorS_ExposureModeType_AutoPHDR;
		ExposureBasicSetting.ExposureNumber = 1;
		ExposureBasicSetting.AutoHDRPriority = 3;
		ExposureBasicSetting.AutoPHDRQuality = 90;
		Sensor->SetExposureBasicSetting(ExposureBasicSetting);
		
		break;
	case 0:
		goto ExitPos;
		break;
	default:
		printf("Invalid input, please enter again. \n\n");
		goto SetDataFormat;
		break;
	}

StartSnapPose:
	
	Sensor->SetTriggerSource(MPSizectorS_TriggerSourceType_KeepRunning);//设定触发源。  SetTriggerSource  连续拍摄，当一次拍摄完成后自动开始下一次拍摄。 
	Start_T = clock();
	system("Pause");

ExitPos:

	delete Sensor;
	return 0;

}





//显示点云
boost::shared_ptr<pcl::visualization::PCLVisualizer>simpleVIS(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer>  viewer (new pcl::visualization::PCLVisualizer("3D Viewer"));


    //viewer->addPointCloud<pcl::PointXYZRGBA>(cloud, "sample cloud");
	//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 0.5, "sample cloud");
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZRGBA> fildcolor(cloud,"z");
	viewer->addPointCloud(cloud,fildcolor,"sample cloud");
	viewer->updatePointCloud(cloud,fildcolor,"sample cloud");
    viewer->addCoordinateSystem(1.0);//坐标系
	viewer->resetCamera();
    //viewer->initCameraParameters();
	

    return(viewer);

}


void DeviceStateChangeCallBack(MPSizectorS_DeviceStateType DeviceState)
{	//设备状态改变回调函数类型。 MPSizectorS_DeviceStateType
	static MPSizectorS_DeviceStateType LastDeviceState = MPSizectorS_DeviceStateType_Disconnected;
	if (SHOW_DETAIL_STATE)
	{
		if (DeviceState == MPSizectorS_DeviceStateType_Disconnected) printf("Device Disconnected\r\n");
		else if (DeviceState == MPSizectorS_DeviceStateType_UnderInit) printf("Device UnderInit\r\n");
		else if (DeviceState == MPSizectorS_DeviceStateType_StandBy) printf("Device StandBy\r\n");
		else if (DeviceState == MPSizectorS_DeviceStateType_UnderExposure) printf("Device UnderExposure\r\n");
		else if (DeviceState == MPSizectorS_DeviceStateType_UnderTransfer) printf("Device UnderTransfer\r\n");
	}
	else
	{
		if (DeviceState == MPSizectorS_DeviceStateType_Disconnected) printf("Device Disconnected\r\n");
		else if (DeviceState == MPSizectorS_DeviceStateType_UnderInit) printf("Device UnderInit\r\n");
		else if ((DeviceState == MPSizectorS_DeviceStateType_StandBy)&&((LastDeviceState == MPSizectorS_DeviceStateType_Disconnected)||(LastDeviceState == MPSizectorS_DeviceStateType_UnderInit))) printf("Device StandBy\r\n");
	}

	LastDeviceState = DeviceState;

}

void LogCallBack(const char* Log)
{
	printf("LOG: %s",Log);
}

void DataCallBack(MPSizectorS_DataFormatType DataFormat, MPSizectorS_DataFrameUndefinedStruct Data)
{
	static clock_t t1 = clock(),t2 = clock();
	static int Cnt;
	unsigned int diffms;
	t2 = clock();
	diffms = (t2-t1) * 1000 / CLOCKS_PER_SEC;
	t1 = t2;

	Cnt++;

	float AvgFPS = (float)Cnt / ((float)(t2 - Start_T) / (float)CLOCKS_PER_SEC);

	if (DataFormat == MPSizectorS_DataFormatType_FloatPointCloud)
	{
		//MPSizectorS_DataPointFloat3DStruct  
		MPSizectorS_DataFrameFloat3DStruct Float3DData = MPSizectorS_Utils::ConvertToFloat3DFrame(Data);// 点云**********************************
		printf("Recieved FloatPointCloud, %ux%u, z = %7.4f mm, cnt = %d, t = %d ms, AvgFPS = %6.2f FPS\r\n", Float3DData.FrameInfo.DataInfo.XPixResolution,  Float3DData.FrameInfo.DataInfo.YPixResolution, Float3DData.Data[Float3DData.FrameInfo.DataInfo.XPixResolution/2 + Float3DData.FrameInfo.DataInfo.YPixResolution / 2 * Float3DData.FrameInfo.DataInfo.XPixResolution].Z, Cnt, diffms, AvgFPS);
		
		//printf("DataNumber: %u\n", _msize(Float3DData.Data));

		Getdatacopy(Float3DData,cloud);
	}
	else if (DataFormat == MPSizectorS_DataFormatType_FixPointCloud)
	{
		MPSizectorS_DataFrameFix3DStruct Fix3DData = MPSizectorS_Utils::ConvertToFix3DFrame(Data);//将回调函数提供的Undefined格式数据，转换为Fix3D格式。
		printf("Recieved FixPointCloud%d, %ux%u, z = %7.4f mm, cnt = %d, t = %d ms, AvgFPS = %6.2f FPS\r\n", Fix3DData.FrameInfo.DataInfo.TSN.SN, Fix3DData.FrameInfo.DataInfo.XPixResolution,  Fix3DData.FrameInfo.DataInfo.YPixResolution, Fix3DData.Data[Fix3DData.FrameInfo.DataInfo.XPixResolution/2 + Fix3DData.FrameInfo.DataInfo.YPixResolution / 2 * Fix3DData.FrameInfo.DataInfo.XPixResolution].Z * Data.FrameInfo.PostProcessSettings.PointScaleSetting.ZIncrement + Data.FrameInfo.PostProcessSettings.PointScaleSetting.Z0Pos, Cnt, diffms, AvgFPS);
	}
	// else if (DataFormat == MPSizectorS_DataFormatType_Image2D)
	// {
	// 	MPSizectorS_DataFrame2DStruct Data2D = MPSizectorS_Utils::ConvertTo2DFrame(Data);
	// 	printf("Recieved 2D x %u, %ux%u, cnt = %d, t = %d ms, AvgFPS = %6.2f FPS\r\n", Data2D.FrameInfo.DataInfo.DataNumber , Data2D.FrameInfo.DataInfo.XPixResolution,  Data2D.FrameInfo.DataInfo.YPixResolution, Cnt, diffms, AvgFPS);
	// }
}
int Getdatacopy(MPSizectorS_DataFrameFloat3DStruct Float3DData, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud)
{
	int width = Float3DData.FrameInfo.DeviceInfo.SensorWidth;
	int height = Float3DData.FrameInfo.DeviceInfo.SensorHeight;
	long cloudsize = width*height;
	pointcloud->resize(cloudsize);
	for (long i =0; i< cloudsize; ++i)
	{
		pointcloud->points[i].x = Float3DData.Data[i].X;
		pointcloud->points[i].y = Float3DData.Data[i].Y;
		pointcloud->points[i].z = Float3DData.Data[i].Z;
		pointcloud->points[i].r = Float3DData.Data[i].Gray;
		pointcloud->points[i].g = Float3DData.Data[i].Gray;
		pointcloud->points[i].b = Float3DData.Data[i].Gray;
		pointcloud->points[i].a = 255;
	}
	//pcl::PCDWriter writer;
	//writer.writeASCII("pp.pcd",*cloud);
	return 0;
}

#endif